//
// Created by xujingyi on 2021/5/21.
//
#include "opencv2/video/tracking.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <Serial.h>
#include "solve/Predictor.h"
#include "armor_finder.h"
#include "Serial.h"
#include "solve/trajectory_resolve.h"
extern rm::Setter rm_setter;
extern rm::ArmorFinder armor_finder;
extern Serial serial;
extern int fps;
extern cv::Mat show_pic;
namespace rm {
#if SHOOTER_MODE==DOWN_SHOOTER
    void KalmanFilter::init(int s) {//yaw:x
        _mode=s;
        if (s == 1) {
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);//F
            Kf->transitionMatrix.at<float>(0, 1) = 0.000010f;//delta t
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 30.0f;//Q max be fast
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);//R
            Kf->measurementNoiseCov.at<float>(0, 0) = 2000.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;//P
            return;
        }
        else if (s == 2) {//pitch:y
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
            Kf->transitionMatrix.at<float>(0, 1) = 0.01f;
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 100.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 3) {//chassis location
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            int d = 3;
            int m = 3;
            float dt=1000.0f/106.0f;
            Kf->transitionMatrix = (cv::Mat_<float >(d, m) << 1.0, dt, dt*dt,
                    0.0, 1.0, dt,
                    0.0, 0.0, 1.0);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
//            Kf->transitionMatrix.at<float>(0, 1) = 0.0015f;
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 100.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) = 1000.0f;
            Kf->errorCovPost = cv::Mat::eye(2, 2, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 4) {//x,y,z
            int d = 6;
            int m = 3;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0, 0.0, 0.0, 1.0,
                    0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 30.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) = 2100.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 5) {//yaw
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);//F
            Kf->transitionMatrix.at<float>(0, 1) = 0.805f;//delta t
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 20.0f;//Q max be fast
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);//R
            Kf->measurementNoiseCov.at<float>(0, 0) = 20.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 20.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;//P
            return;
        }
        else if (s == 6) {//pitch
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
            Kf->transitionMatrix.at<float>(0, 1) = 0.705f;
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 20.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;
            return;
        }
    }

    KalmanFilter::KalmanFilter(int s) {//yaw:x
        if (s == 1) {
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(1);
        }
        else if (s == 2) {//pitch:y
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(2);
        }
        else if (s == 3) {//chassis location
            Kf = new cv::KalmanFilter(3, 3, 0);
            init(3);
        }
        else if (s == 4) {//x,y,z
            int d = 6;
            int m = 3;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(4);
        }
        else if (s == 5) {//yaw
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(5);
        }
        else if (s == 6) {//pitch
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(6);
        }
    }

    cv::Mat KalmanFilter::predict(cv::Mat mes,int n,const float delay_t) {
        Kf->correct(mes);
        cv::Mat pre = Kf->predict();
        if(n==0) return pre;
        if(n==1){
            pre.at<float>(0)=pre.at<float>(0)+pre.at<float>(2)*time2frame(delay_t);
            pre.at<float>(1)=pre.at<float>(1)+pre.at<float>(3)*time2frame(delay_t);
            return pre;
        }
    }

#if PRE_MODE==PIXEL_MODE
    cv::Point2f KalmanFilter::predict(cv::Point2f mes,int n,const float delay_t) {
        cv::Mat temp = cv::Mat::zeros(2, 1, CV_32FC1);
        temp.at<float>(0, 0) = mes.x;
        temp.at<float>(1, 0) = mes.y;
        Kf->correct(temp);
        cv::Mat pre = Kf->predict();
        p1 = p2;
        p2 = cv::Point2f(pre.at<float>(0, 0), pre.at<float>(1, 0));
        if(n==0) return p2;
        if(n==1){
            p2 = p2 + cv::Point2f(p2.y* time2frame(delay_t), 0);
            return p2;
        }
    }

    void KalmanFilter::init(bool s) {//dx,dy
        Kf->statePre=cv::Scalar::all(0);
        Kf->statePost==cv::Scalar::all(0);
        Kf->controlMatrix=cv::Scalar::all(0);
        Kf->measurementMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->transitionMatrix.at<float>(0, 1) = 0.01f;
        Kf->transitionMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->processNoiseCov = cv::Mat::eye(2, 2, CV_32FC1) * 2500.0f;
        Kf->measurementNoiseCov = cv::Mat::eye(2, 2, CV_32FC1) * 5000.0f;
    }

    KalmanFilter::KalmanFilter(bool s) {//dx,dy
        Kf = new cv::KalmanFilter(2, 2, 0);
        init(s);
    }

    cv::Point2f rm::Predictor::operator()(cv::Point2f center,const float distance,const float delay_t) {

        //bug(PitchYawDistance.z*sin(PTZvap/(5730.0)));
        //We don't predict targets if the targets appear suddenly or are changed suddenly;
        float xtemp = 1000, ytemp = 1000;
        float lamuda = FOCUS_PIXAL;
        float f = FOCUS;
        cv::Point2f tempP(0, 0);
        cv::Point2f tempY(0, 0);
        cv::Point2f pyangle= pixelToAngle(center);
        //Prediction Based on PTZva
        cv::Mat measurement4 = cv::Mat::zeros(2, 1, CV_32FC1);
        measurement4.at<float>(0, 0) =
                pyangle.x +
                (float) serial.qPitches.mean();
        measurement4.at<float>(1, 0) =
                (float) (serial.qP_speeds[-1] + serial.qP_speeds[-2] + serial.qP_speeds[-3]) / (3.0f);

        cv::Mat result4 = KFp->predict(measurement4);
        tempP.x = tan((result4.at<float>(0, 0) - (float) serial.qPitches.mean()) / ANGLE_PER_RAD)   * f * lamuda +
                  IMAGE_CENTER_Y;//angleToPixel
        float deltaP = 1.0f * (center.y - tempP.x);
        tempP.x =(float) (center.y + deltaP) ;
        tempP.y = (float) (0);
        tempP = UltimateKFp->predict(tempP);

        cv::Mat measurement3 = cv::Mat::zeros(2, 1, CV_32FC1);
        measurement3.at<float>(0, 0) =
                pyangle.y +
                (float) serial.qYaws[-1];
        measurement3.at<float>(1, 0) =
                (float) (serial.qY_speeds[-1]);

        cv::Mat result3 = KFy->predict(measurement3);
        tempY.x = tan((result3.at<float>(0, 0) - serial.qYaws[-1]) / ANGLE_PER_RAD) * f * lamuda + IMAGE_CENTER_X;
        float deltaY = 0.6f * (center.x - tempY.x);
        tempY.x = center.x + deltaY;
        tempY.y = (float) (0);
        tempY = UltimateKFy->predict(tempY);

        if (Boxposes.size() == posSize) {
            ytemp = tempP.x;
            xtemp = tempY.x;
        } else {
            xtemp = center.x;
            ytemp = center.y;
        }
        return cv::Point2f(xtemp,ytemp);
    }
#elif PRE_MODE==COORD_MODE

    cv::Point3f KalmanFilter::predict(const ChassisState chassis_state,cv::Point3f mes,int n,const float delay_t) {
        cv::Mat temp = cv::Mat::zeros(3, 1, CV_32FC1);
        temp.at<float>(0, 0) = mes.x;
        temp.at<float>(1, 0) = mes.y;
        temp.at<float>(2, 0) = mes.z;

        Kf->correct(temp);

        cv::Mat pre = Kf->predict();

        char cspeedNacc[100];
        sprintf(cspeedNacc,"speed:%.1f acc:%.1f",pre.at<float>(1, 0)*1000,pre.at<float>(2, 0)*1000000.0f);//+0.5*p3f2.z*delay_t*delay_t
        cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,3,COLOR_WHITE);
        p3f1 = p3f2;
        p3f2 = cv::Point3f(pre.at<float>(0, 0), pre.at<float>(1, 0),pre.at<float>(2, 0));
        float aver_acc=serial.qChss_acc.mean();
        if(n==0) {
            return p3f2;
        }
        if(n==1){
            if((mes.z>0&&(-serial.rxpackage.chassis_speed.type_short<500))||(mes.z<0&&(serial.rxpackage.chassis_speed.type_short<500))) p3f2.x=mes.x;
            else p3f2 = p3f2 + cv::Point3f((p3f2.y) *delay_t+0.5*p3f2.z*delay_t*delay_t, p3f2.z*delay_t,0);
//p3f2 = p3f2 + cv::Point3f(p3f2.y * delay_t+0.5*p3f2.z/2000.0f*delay_t*delay_t, p3f2.z/2000.0f*delay_t,0);
            char cspeedNacc[100];
            sprintf(cspeedNacc,"loc:%.1f speed:%.3f acc:%.10f",p3f2.x,p3f2.y,p3f2.z);//+0.5*p3f2.z*delay_t*delay_t
            cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y+100),1,2,COLOR_WHITE);
            return p3f2;
        }
        if(n==2){
//            p3f2 = p3f2 + cv::Point3f(p3f2.y* time2frame(delay_t), p3f2.z*time2frame(delay_t),0);
            if(((aver_acc<0&&serial.rxpackage.chassis_speed.type_short<500))||((aver_acc>0&&-serial.rxpackage.chassis_speed.type_short<500))) p3f2=p3f2 + cv::Point3f(2*(p3f2.z) * delay_t+2.0*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);
            else p3f2 = p3f2 + cv::Point3f((p3f2.y) * delay_t+0.5*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);
//            p3f2 = p3f2 + cv::Point3f((p3f2.y+p3f2.z) * delay_t+0.5*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);

//p3f2 = p3f2 + cv::Point3f(p3f2.y * delay_t+0.5*p3f2.z/2000.0f*delay_t*delay_t, p3f2.z/2000.0f*delay_t,0);
            char cspeedNacc[100];
            sprintf(cspeedNacc,"loc:%.1f speed:%.3f acc:%.10f",p3f2.x,p3f2.y,p3f2.z);//+0.5*p3f2.z*delay_t*delay_t
            cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y+100),1,2,COLOR_WHITE);
            return p3f2;
        }
    }

/*!
 *
 * @param armor_state 装甲模式
 * @param coord 世界坐标
 * @param n 0:普通模式 1：补偿模式
 * @param delay_t 子弹飞行时间+延迟时间 单位:ms
 * @return
 */
    cv::Point3f KalmanFilter::predict(const ArmorState armor_state,cv::Point3f coord,const int n,const float delay_t) {
        cv::Mat measurement = cv::Mat::zeros(3,1,CV_32F);
        cv::Mat pre;
        switch(armor_state){
            case TRACK_ARMOR:
                measurement.at<float>(0) = coord.x;
                measurement.at<float>(1) = coord.y;
                measurement.at<float>(2) = coord.z;
                Kf->correct(measurement);
                pre = Kf->predict();
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                break;
            case CHANGE_ARMOR:
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                Kf->statePost.at<float>(0)=coord.x;
                Kf->statePost.at<float>(1)=coord.y;
                Kf->statePost.at<float>(2)=coord.z;
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                pre = Kf->predict();
                break;
            case CHANGE_ENEMY:
                Kf->statePost.at<float>(0)=coord.x;
                Kf->statePost.at<float>(1)=coord.y;
                Kf->statePost.at<float>(2)=coord.z;
                Kf->statePost.at<float>(3)=0;
                Kf->statePost.at<float>(4)=0;
                Kf->statePost.at<float>(5)=0;
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                pre = Kf->predict();
                break;
//            case TRACK_GYRO:
//                Kf->statePost.at<float>(0)=coord.x;
//                Kf->statePost.at<float>(1)=coord.y;
//                Kf->statePost.at<float>(2)=coord.z;
//                Kf->statePost.at<float>(3)=0;
//                Kf->statePost.at<float>(4)=0;
//                Kf->statePost.at<float>(5)=0;
        }
        if(n==0){
            p3f1 = p3f2;
            p3f2 = cv::Point3f(pre.at<float>(0, 0), pre.at<float>(1, 0),pre.at<float>(2, 0));
            return p3f2;
        }
        if(n==1){
            float dt=time2frame(delay_t);
            p3f1 = p3f2;
            p3f2=_pos+_vel*dt;
//            p3f2 = cv::Point3f(pre.at<float>(0, 0)+pre.at<float>(3, 0)*dt, pre.at<float>(1, 0)+pre.at<float>(4, 0)*dt,pre.at<float>(2, 0)+pre.at<float>(5, 0)*dt);
            return p3f2;
        }
    }
extern cv::Mat show_pic;
    cv::Point2f rm::Predictor::operator()(const ArmorState armor_state,const ChassisState chassis_state,cv::Point3f coord,const float delay_t){
        float c_loc=0,acc=(getTimeIntervalms(serial.t,serial.last_t)>1)?((serial.qChss_spd[-1]-serial.qChss_spd[-2])/getTimeIntervalms(serial.t,serial.last_t)):serial.qChss_acc.mean();

        if(!serial.rxpackage.shake_mode) c_loc=(KFl->predict(chassis_state,cv::Point3f(serial.qChassis_locations[-1],serial.qChss_spd[-1],acc),1,40.0*delay_t)).x;
        else {
            c_loc=(KFl->predict(chassis_state,cv::Point3f(serial.qChassis_locations[-1],serial.qChss_spd[-1],acc),2,15.0*delay_t)).x;
        }
        cv::Point3f world_coord=KFc->predict(armor_state,coord,1,10*delay_t);
//                float p=(KFp->predict(cv::Point2f(serial.qPitches[-1],0),0,1*delay_t)).x;
//        float y=(KFy->predict(cv::Point2f(serial.qYaws[-1],0),0,0.7*delay_t)).x;
//        float p=(KFp->predict(cv::Point2f(serial.qPitches[-1],serial.qP_speeds[-1]),0,1*delay_t)).x;
//        float y=(KFy->predict(cv::Point2f(serial.qYaws[-1],serial.qY_speeds[-1]),0,0.7*delay_t)).x;
        float p=serial.qPitches[-1];
        float y=serial.qYaws[-1];
        cv::Point2f pyangle=cv::Point2f(p,y);
        Eigen::Matrix<float,3,3> R_back,R_vec;
        calcRotMatrix(R_vec,R_back,pyangle);
        cv::Point3f refer_coord=antiWorldCoordinateResolver(world_coord,R_back,c_loc);
        return angleToPixel(referCoordinate2Angle(refer_coord));
    }

#endif
#else
    void KalmanFilter::init(int s) {//yaw:x
        _mode=s;
        if (s == 1) {
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);//F
            Kf->transitionMatrix.at<float>(0, 1) = 0.10f;//delta t
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 30.0f;//Q max be fast
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);//R
            Kf->measurementNoiseCov.at<float>(0, 0) = 2000.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;//P
            return;
        }
        else if (s == 2) {//pitch:y
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
            Kf->transitionMatrix.at<float>(0, 1) = 0.01f;
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 100.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 3) {//chassis location
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            int d = 3;
            int m = 3;
            float dt=1000.0f/106.0f;
            Kf->transitionMatrix = (cv::Mat_<float >(d, m) << 1.0, dt, 0.5*dt*dt,
                    0.0, 1.0, dt,
                    0.0, 0.0, 1.0);
//            Kf->transitionMatrix = (cv::Mat_<float >(d, m) << 1.0, dt, dt*dt,
//                    0.0, 1.0, dt,
//                    0.0, 0.0, 1.0);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
//            Kf->transitionMatrix.at<float>(0, 1) = 0.0015f;
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 30.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 500.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) = 1000.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 4) {//x,y,z
            int d = 6;
            int m = 3;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost=cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0, 0.0, 0.0, 1.0,
                    0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 30.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) = 2100.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 2.0f;
            return;
        }
        else if (s == 5) {//yaw
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);//B
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);//F
            Kf->transitionMatrix.at<float>(0, 1) = 0.105f;//delta t
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 20.0f;//Q max be fast
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);//R
            Kf->measurementNoiseCov.at<float>(0, 0) = 2000.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;//P
            return;
        }
        else if (s == 6) {//pitch
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->controlMatrix=cv::Scalar::all(0);
            Kf->measurementMatrix = cv::Mat::eye(m, m, CV_32FC1);
            Kf->transitionMatrix = cv::Mat::eye(d, m, CV_32FC1);
            Kf->transitionMatrix.at<float>(0, 1) = 0.105f;
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 20.0f;
            Kf->measurementNoiseCov = cv::Mat::zeros(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 2100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
            Kf->errorCovPost = cv::Mat::eye(d, m, CV_32FC1) * 2.0f;
            return;
        }
    }

    KalmanFilter::KalmanFilter(int s) {//yaw:x
        if (s == 1) {
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(1);
        }
        else if (s == 2) {//pitch:y
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(2);
        }
        else if (s == 3) {//chassis location
            Kf = new cv::KalmanFilter(3, 3, 0);
            init(3);
        }
        else if (s == 4) {//x,y,z
            int d = 6;
            int m = 3;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(4);
        }
        else if (s == 5) {//yaw
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(5);
        }
        else if (s == 6) {//pitch
            int d = 2;
            int m = 2;
            Kf = new cv::KalmanFilter(d, m, 0);
            init(6);
        }
    }

    cv::Point3f KalmanFilter::predict(const ChassisState chassis_state,cv::Point3f mes,int n,const float delay_t) {
        cv::Mat temp = cv::Mat::zeros(3, 1, CV_32FC1);
        temp.at<float>(0, 0) = mes.x;
        temp.at<float>(1, 0) = mes.y;
        temp.at<float>(2, 0) = mes.z;

        Kf->correct(temp);

        cv::Mat pre = Kf->predict();

//        char cspeedNacc[100];
//        sprintf(cspeedNacc,"speed:%.1f acc:%.1f",pre.at<float>(1, 0)*1000,pre.at<float>(2, 0)*1000000.0f);//+0.5*p3f2.z*delay_t*delay_t
//        cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,3,COLOR_WHITE);
        p3f1 = p3f2;
        p3f2 = cv::Point3f(pre.at<float>(0, 0), pre.at<float>(1, 0),pre.at<float>(2, 0));
        float aver_acc=serial.qChss_acc.mean();
        if(n==0) {
            return p3f2;
        }
        if(n==1){
            if((mes.z>0&&(-serial.rxpackage.chassis_speed.type_short<200))||(mes.z<0&&(serial.rxpackage.chassis_speed.type_short<200))) p3f2.x=mes.x;
            else p3f2 = p3f2 + cv::Point3f((p3f2.y) *delay_t+0.5*p3f2.z*delay_t*delay_t, p3f2.z*delay_t,0);
//p3f2 = p3f2 + cv::Point3f(p3f2.y * delay_t+0.5*p3f2.z/2000.0f*delay_t*delay_t, p3f2.z/2000.0f*delay_t,0);
//            char cspeedNacc[100];
//            sprintf(cspeedNacc,"loc:%.1f speed:%.3f acc:%.10f",p3f2.x,p3f2.y,p3f2.z);//+0.5*p3f2.z*delay_t*delay_t
//            cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y+100),1,2,COLOR_WHITE);
            return p3f2;
        }
        if(n==2){
//            p3f2 = p3f2 + cv::Point3f(p3f2.y* time2frame(delay_t), p3f2.z*time2frame(delay_t),0);
            if(((aver_acc<0&&serial.rxpackage.chassis_speed.type_short<300))||((aver_acc>0&&-serial.rxpackage.chassis_speed.type_short<300))) p3f2=p3f2 + cv::Point3f(2*(p3f2.z) * delay_t+2.0*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);
            else p3f2 = p3f2 + cv::Point3f((p3f2.y+1.5*p3f2.z) * delay_t+0.5*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);
//            p3f2 = p3f2 + cv::Point3f((p3f2.y+p3f2.z) * delay_t+0.5*p3f2.z*delay_t*delay_t, 0.5*p3f2.z*delay_t,0);

//p3f2 = p3f2 + cv::Point3f(p3f2.y * delay_t+0.5*p3f2.z/2000.0f*delay_t*delay_t, p3f2.z/2000.0f*delay_t,0);
//            char cspeedNacc[100];
//            sprintf(cspeedNacc,"loc:%.1f speed:%.3f acc:%.10f",p3f2.x,p3f2.y,p3f2.z);//+0.5*p3f2.z*delay_t*delay_t
//            cv::putText(show_pic,cspeedNacc,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y+100),1,2,COLOR_WHITE);
            return p3f2;
        }
    }

    cv::Mat KalmanFilter::predict(cv::Mat mes,int n,const float delay_t) {
        Kf->correct(mes);
        cv::Mat pre = Kf->predict();
        if(n==0) return pre;
        if(n==1){
            pre.at<float>(0)=pre.at<float>(0)+pre.at<float>(2)*time2frame(delay_t);
            pre.at<float>(1)=pre.at<float>(1)+pre.at<float>(3)*time2frame(delay_t);
            return pre;
        }
    }

#if PRE_MODE==PIXEL_MODE
    void KalmanFilter::init(bool s) {//dx,dy
        Kf->statePre=cv::Scalar::all(0);
        Kf->statePost==cv::Scalar::all(0);
        Kf->controlMatrix=cv::Scalar::all(0);
        Kf->measurementMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->transitionMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->processNoiseCov = cv::Mat::eye(2, 2, CV_32FC1) * 2500.0f;
        Kf->measurementNoiseCov = cv::Mat::eye(2, 2, CV_32FC1) * 5000.0f;
    }

    KalmanFilter::KalmanFilter(bool s) {//dx,dy
        Kf = new cv::KalmanFilter(2, 2, 0);
        init(s);
    }

    cv::Point2f rm::Predictor::operator()(PreState state,cv::Point2f center,const float distance,const float delay_t) {

        //bug(PitchYawDistance.z*sin(PTZvap/(5730.0)));
        //We don't predict targets if the targets appear suddenly or are changed suddenly;
        float xtemp = 1000, ytemp = 1000;
        float lamuda = FOCUS_PIXAL;
        float f = FOCUS;
        cv::Point2f tempP(0, 0);
        cv::Point2f tempY(0, 0);
        cv::Point2f pyangle= pixelToAngle(center);
        //Prediction Based on PTZva
        cv::Mat measurement4 = cv::Mat::zeros(2, 1, CV_32FC1);
        measurement4.at<float>(0, 0) =
                pyangle.x +
                (float) serial.qPitches.mean();
        measurement4.at<float>(1, 0) =
                (float) (serial.qP_speeds[-1] + serial.qP_speeds[-2] + serial.qP_speeds[-3]) / (3.0f);

        cv::Mat result4 = KFp->predict(measurement4);
        tempP.x = tan((result4.at<float>(0, 0) - (float) serial.qPitches.mean()) / ANGLE_PER_RAD)   * f * lamuda +
                  IMAGE_CENTER_Y;//angleToPixel
        float deltaP = 1.0f * (center.y - tempP.x);
        tempP.x =(float) (center.y + deltaP) ;
        tempP.y = (float) (0);
        tempP = UltimateKFp->predict(tempP);

        cv::Mat measurement3 = cv::Mat::zeros(2, 1, CV_32FC1);
        measurement3.at<float>(0, 0) =
                pyangle.y +
                (float) serial.qYaws.mean();
        measurement3.at<float>(1, 0) =
                (float) (serial.qY_speeds[-1] + serial.qY_speeds[-2] + serial.qY_speeds[-3]) / (3.0f);

        cv::Mat result3 = KFy->predict(measurement3);
        tempY.x = tan((result3.at<float>(0, 0) - serial.qYaws.mean()) / 57.29f) * f * lamuda + IMAGE_CENTER_X;
        float deltaY = 1.7f * (center.x - tempY.x);
        tempY.x = center.x + deltaY;
        tempY.y = (float) (0);
        tempY = UltimateKFy->predict(tempY);

        if (Boxposes.size() == posSize) {
            ytemp = tempP.x;
            xtemp = tempY.x;
        } else {
            xtemp = center.x;
            ytemp = center.y;
        }
        return cv::Point2f(xtemp,ytemp);
    }
#elif PRE_MODE==COORD_MODE
/*!
 *
 * @param armor_state 装甲模式
 * @param coord 世界坐标
 * @param n 0:普通模式 1：补偿模式
 * @param delay_t 子弹飞行时间+延迟时间 单位:ms
 * @return
 */
    cv::Point3f KalmanFilter::predict(const ArmorState armor_state,cv::Point3f coord,const int n,const float delay_t) {
        cv::Mat measurement = cv::Mat::zeros(3,1,CV_32F);
        cv::Mat pre;
        switch(armor_state){
            case TRACK_ARMOR:
                measurement.at<float>(0) = coord.x;
                measurement.at<float>(1) = coord.y;
                measurement.at<float>(2) = coord.z;
                Kf->correct(measurement);
                pre = Kf->predict();
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                break;
            case CHANGE_ARMOR:
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                Kf->statePost.at<float>(0)=coord.x;
                Kf->statePost.at<float>(1)=coord.y;
                Kf->statePost.at<float>(2)=coord.z;
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                pre = Kf->predict();
                break;
            case CHANGE_ENEMY:
                Kf->statePost.at<float>(0)=coord.x;
                Kf->statePost.at<float>(1)=coord.y;
                Kf->statePost.at<float>(2)=coord.z;
                Kf->statePost.at<float>(3)=0;
                Kf->statePost.at<float>(4)=0;
                Kf->statePost.at<float>(5)=0;
                _pos=cv::Point3f( Kf->statePost.at<float>(0, 0), Kf->statePost.at<float>(1, 0), Kf->statePost.at<float>(2, 0));
                _vel=cv::Point3f( Kf->statePost.at<float>(3, 0), Kf->statePost.at<float>(4, 0), Kf->statePost.at<float>(5, 0));
                pre = Kf->predict();
                break;
//            case TRACK_GYRO:
//                Kf->statePost.at<float>(0)=coord.x;
//                Kf->statePost.at<float>(1)=coord.y;
//                Kf->statePost.at<float>(2)=coord.z;
//                Kf->statePost.at<float>(3)=0;
//                Kf->statePost.at<float>(4)=0;
//                Kf->statePost.at<float>(5)=0;
        }
        if(n==0){
            p3f1 = p3f2;
            p3f2 = cv::Point3f(pre.at<float>(0, 0), pre.at<float>(1, 0),pre.at<float>(2, 0));
            return p3f2;
        }
        if(n==1){
            float dt=time2frame(delay_t);
            p3f1 = p3f2;
            p3f2=_pos+_vel*dt;
//            p3f2 = cv::Point3f(pre.at<float>(0, 0)+pre.at<float>(3, 0)*dt, pre.at<float>(1, 0)+pre.at<float>(4, 0)*dt,pre.at<float>(2, 0)+pre.at<float>(5, 0)*dt);
            return p3f2;
        }
    }

    cv::Point2f rm::Predictor::operator()(const ArmorState armor_state,const ChassisState chassis_state,cv::Point3f coord,const float delay_t){
        float c_loc=0,acc=serial.qChss_acc.mean();

        if(!serial.rxpackage.shake_mode) c_loc=(KFl->predict(chassis_state,cv::Point3f(serial.qChassis_locations[-1],serial.qChss_spd[-1],acc),1,21.5*delay_t)).x;
        else {
            c_loc=(KFl->predict(chassis_state,cv::Point3f(serial.qChassis_locations[-1],serial.qChss_spd[-1],acc),2,10.0*delay_t)).x;
        }
        cv::Point3f world_coord=KFc->predict(armor_state,coord,1,15*delay_t);
//                float p=(KFp->predict(cv::Point2f(serial.qPitches[-1],0),0,1*delay_t)).x;
//        float y=(KFy->predict(cv::Point2f(serial.qYaws[-1],0),0,0.7*delay_t)).x;
//        float p=(KFp->predict(cv::Point2f(serial.qPitches[-1],serial.qP_speeds[-1]),0,1*delay_t)).x;
//        float y=(KFy->predict(cv::Point2f(serial.qYaws[-1],serial.qY_speeds[-1]),0,0.7*delay_t)).x;
        float p=serial.qPitches[-1];
        float y=serial.qYaws[-1];
        cv::Point2f pyangle=cv::Point2f(p,y);
        Eigen::Matrix<float,3,3> R_back,R_vec;
        calcRotMatrix(R_vec,R_back,pyangle);
        cv::Point3f refer_coord=antiWorldCoordinateResolver(world_coord,R_back,c_loc);
        return angleToPixel(referCoordinate2Angle(refer_coord));
    }

#endif
#endif
}

//-----------------------------小陀螺预测类-----------------------

void rm::GyroKalmanFilter::init(int mode){
    if (mode == 1) {//motor_yaw
        int c = 2;
        int m = 2;
        Kf->measurementMatrix = cv::Mat::eye(c, m, CV_32FC1);
        Kf->transitionMatrix = cv::Mat::eye(c, m, CV_32FC1);
        Kf->transitionMatrix.at<float>(0, 1) = 0.01f;
        Kf->processNoiseCov = cv::Mat::eye(c, m, CV_32FC1) * 50.0f;//max be fast
        Kf->measurementNoiseCov = cv::Mat::zeros(c, m, CV_32FC1);
        Kf->measurementNoiseCov.at<float>(0, 0) = 2000.0f;
        Kf->measurementNoiseCov.at<float>(1, 1) = 500.0f;
        Kf->errorCovPost = cv::Mat::eye(c, m, CV_32FC1) * 2.0f;
    }
    if (mode == 2) {//pixel_x
        Kf->measurementMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->transitionMatrix = cv::Mat::eye(2, 2, CV_32FC1);
        Kf->processNoiseCov = cv::Mat::eye(2, 2, CV_32FC1) * 5000.0f;
        Kf->measurementNoiseCov = cv::Mat::zeros(2, 2, CV_32FC1) * 10000.0f;
    }
}

rm::GyroKalmanFilter::GyroKalmanFilter(int mode) {//yaw:x
    if (mode == 1) {//motor_yaw
        int d = 2;
        int m = 2;
        Kf = new cv::KalmanFilter(d, m, 0);
init(1);
    }
    if (mode == 2) {//pixel_x
        Kf = new cv::KalmanFilter(2, 2, 0);
init(2);
    }
}

cv::Mat rm::GyroKalmanFilter::GyroKalmanFilter::predict(cv::Mat mes) {
    Kf->correct(mes);
    cv::Mat pre = Kf->predict();
    return pre;
}

cv::Point2f rm::GyroKalmanFilter::predict(cv::Point2f mes,int n,float delay_t) {
    cv::Mat temp = cv::Mat::zeros(2, 1, CV_32FC1);
    temp.at<float>(0, 0) = mes.x;
//    if(prepre)
    temp.at<float>(1, 0) = mes.y;
    Kf->correct(temp);
    cv::Mat pre = Kf->predict();
    p1 = p2;
    p2 = cv::Point2f(pre.at<float>(0, 0), pre.at<float>(1, 0));
    if(n==0) return p2;
    if(n==1){
        p2 = p2 + cv::Point2f(pre.at<float>(1, 0)* time2frame(delay_t), 0);
        return p2;
    }
}

void rm::GyroPredictor::init() {
    kf.init(2, 1);
    kf.transitionMatrix=(cv::Mat_<float>(2, 2) <<1.0, 1.0, 0, 1.0);
    setIdentity(kf.measurementMatrix);
    setIdentity(kf.controlMatrix,cv::Scalar::all(0));
    setIdentity(kf.processNoiseCov,cv::Scalar::all(1));
    setIdentity(kf.measurementNoiseCov,cv::Scalar::all(1000));
    setIdentity(kf.errorCovPost,cv::Scalar::all(1));

    KFy->init(1);
    UltimateKFy->init(2);
}

rm::GyroPredictor::GyroPredictor() {
    KFy = new GyroKalmanFilter(1);//yaw
    UltimateKFy = new GyroKalmanFilter(2);//dx
    init();
}

void rm::GyroPredictor::correct(double angle) {
    if(abs(angle-pre_pos)>2){
        kf.statePost.at<float>(0)=angle;
    }
    else{
        measurement = cv::Mat::zeros(1,1,CV_32F);
        measurement.at<float>(0) = angle;
        kf.correct(measurement);
    }
}


/*!
 *
 * @param pixel_x 像素坐标x
 * @param angle eular绕z轴角度
 * @return cv::Point2f(预测pixel_x,预测eular角)
 */
extern Serial serial;
cv::Point2f rm::GyroPredictor::predict(float pixel_x,float angle,int n,int delay_t) {
    last_pre_x=UltimateKFy->Kf->statePost.at<float>(0);
    correct(angle);
    kf.predict();
    pre_pos=kf.statePost.at<float>(0);
    v=kf.statePost.at<float>(1);

    float xtemp = 1000, ytemp = 1000;
    float lamuda = FOCUS_PIXAL;
    float f = FOCUS;
    cv::Point2f tempY(0, 0);
    //Prediction Based on PTZva

    cv::Mat measurement3 = cv::Mat::zeros(2, 1, CV_32FC1);
    measurement3.at<float>(0, 0) =
            atan((pixel_x - IMAGE_CENTER_X) / (lamuda * f)) * ANGLE_PER_RAD +
            (float) (serial.qYaws[-1]+serial.qYaws[-2]+serial.qYaws[-3])/ (3.1f);
    measurement3.at<float>(1, 0) =
            (float) (serial.qY_speeds[-1] + serial.qY_speeds[-2] + serial.qY_speeds[-3]) / (3.1f);

    cv::Mat result3 = KFy->predict(measurement3);
    tempY.x = tan((result3.at<float>(0, 0) - serial.qYaws.mean()) / ANGLE_PER_RAD) * f * lamuda + IMAGE_CENTER_X;

    float deltaY = 1.7f * (pixel_x - tempY.x);
    tempY.x = pixel_x + deltaY;
    tempY.y = (float) (0);
    tempY = UltimateKFy->predict(tempY,1,delay_t);

    if(n==0){
        return cv::Point2f(tempY.x,angle);
    }
    return cv::Point2f(tempY.x,pre_pos+v*time2frame(delay_t));
}

